#include <buola/cv/coccupancyoctree.h>

namespace buola { namespace cvi {

img::CImage_gray8 get_height_map(const COccupancyOcTree &pTree,const CSize_i &pSize,float /*pTableZ*/)
{
    img::CImage_gray8 lImage(pSize);

    mat::CVec3f lMin,lMax;

    std::tie(lMin,lMax)=get_minmax(pTree);

    int lZRes=50;
    float lValues[51];

    float lXStep=(lMax.x()-lMin.x())/pSize.x;
    float lYStep=(lMax.y()-lMin.y())/pSize.y;
    float lZStep=(lMax.z()-lMin.z())/lZRes;

    float x=lMin.x();
    for(int i=0;i<pSize.x;i++,x+=lXStep)
    {
        float y=lMin.y();
        for(int j=0;j<pSize.y;j++,y+=lYStep)
        {
            bool lHaveValid=false;
            float z=lMin.z();
            int lNOccEmpty=0;
            for(int k=0;k<51;k++,z+=lZStep)
            {
                auto lNode=pTree.Search({x,y,z});
                if(lNode&&lNode->Value()!=0.0f)
                {
                    lNOccEmpty++;
                    lValues[k]=lNode->Value();
                    if(lValues[k]>0.0f)
                        lHaveValid=true;
                }
                else
                {
                    lValues[k]=0.0;
                }

                if(i==110&&j==101)
                    msg_info() << lValues[k] << "\n";
            }

            if(lHaveValid)
            {
                int lLowestCostK=0;
                float lMinCost=std::numeric_limits<float>::max();

                z=lMin.z();
                for(int k=0;k<50;k++,z+=lZStep)
                {
                    float lBelowSum=0;

                    for(int l=k-1;l>=0;l--)
                    {
                        if(lValues[l]>0.0f) //occupied
                            lBelowSum+=1;
                        else if(lValues[l]<0.0f) //free
                            lBelowSum-=1;
                    }

                    float lAboveSum=0;

                    for(int l=k+1;l<51;l++)
                    {
                        if(lValues[l]>0.0f) //occupied
                            lAboveSum+=1;
                        else if(lValues[l]<0.0f) //free
                            lAboveSum-=1;
                    }

                    float lCostValue=lAboveSum-lBelowSum;
                    if(lCostValue<lMinCost)
                    {
                        lMinCost=lCostValue;
                        lLowestCostK=k;
                    }
                }

                lImage(i,j)=5*lLowestCostK;
            }
            else
            {
                lImage(i,j)=0;
            }
        }
    }

    return lImage;
}

mat::CMat_f get_height_map_mat(const COccupancyOcTree &pTree,const CSize_i &pSize,
                               float pMinX,float pMaxX,float pMinY,float pMaxY,float pMinZ,float pMaxZ)
{
    mat::CMat_f lMat(pSize.x,pSize.y);

    int lZRes=100;
    float lValues[lZRes+1];

    float lXStep=(pMaxX-pMinX)/pSize.x;
    float lYStep=(pMaxY-pMinY)/pSize.y;
    float lZStep=(pMaxZ-pMinZ)/lZRes;

    msg_info() << "lZStep=" << lZStep << "\n";
    
    float x=pMinX;
    for(int i=0;i<pSize.x;i++,x+=lXStep)
    {
        float y=pMinY;
        for(int j=0;j<pSize.y;j++,y+=lYStep)
        {
            msg_info() << "a " << x << "," << y << "\n";
            bool lHaveValid=false;
            float z=pMinZ;
            int lNOccEmpty=0;
            for(int k=0;k<=lZRes;k++,z+=lZStep)
            {
                auto lNode=pTree.Search({x,y,z});
                if(lNode&&lNode->Value()!=0.0f)
                {
                    lNOccEmpty++;
                    lValues[k]=lNode->Value();
                    if(lValues[k]>0.0f)
                        lHaveValid=true;
                }
                else
                {
                    lValues[k]=0.0;
                }
            }

            msg_info() << "b\n";
            
            if(lHaveValid)
            {
                msg_info() << "c\n";
                int lLowestCostK=0;
                float lMinCost=std::numeric_limits<float>::max();

                z=pMinZ;
                for(int k=0;k<lZRes;k++,z+=lZStep)
                {
                    float lBelowSum=0;

                    for(int l=k-1;l>=0;l--)
                    {
                        if(lValues[l]>0.0f) //occupied
                            lBelowSum+=1;
                        else if(lValues[l]<0.0f) //free
                            lBelowSum-=1;
                    }

                    float lAboveSum=0;

                    for(int l=k+1;l<=lZRes;l++)
                    {
                        if(lValues[l]>0.0f) //occupied
                            lAboveSum+=1;
                        else if(lValues[l]<0.0f) //free
                            lAboveSum-=1;
                    }

                    float lCostValue=lAboveSum-lBelowSum;
                    if(lCostValue<lMinCost)
                    {
                        lMinCost=lCostValue;
                        lLowestCostK=k;
                    }
                }

                lMat(i,j)=std::max(0.0f,pMinZ+lLowestCostK*lZStep);
            }
            else
            {
                msg_info() << "d\n";
                lMat(i,j)=NAN;
            }
        }
    }

    return lMat;

}

/*namespace cvi*/ } /*namespace buola*/ }
